#include "manual_ego_car/ManualEgoCar.h"

using namespace std::chrono_literals;
using namespace std::string_literals;


ManualEgoCar::ManualEgoCar(/* args */):Node("manual_ego_car_node")
{
    using namespace std::placeholders;

    this->__carlaClient = std::make_shared<carla::client::Client>("127.0.0.1", 2000);
    this->__carlaClient->SetTimeout(10s);
    this->__carlaWorld = std::make_shared<carla::client::World>(this->__carlaClient->GetWorld());
    this->__carlaSpectator = std::make_shared<carla::traffic_manager::ActorPtr>(this->__carlaWorld->GetSpectator());

    // 查找默认出生点
    auto carla_map_ = this->__carlaWorld->GetMap();
    auto spawn_points_ = carla_map_->GetRecommendedSpawnPoints();
    auto spawn_point_ = spawn_points_.at(0);

    // 创建车辆
    auto bpl_ = this->__carlaWorld->GetBlueprintLibrary();
    auto bp_model3_ = bpl_->Find("vehicle.tesla.model3");
    assert (bp_model3_), "找不到蓝图 vehicle.tesla.model3";
    auto act_ = this->__carlaWorld->SpawnActor(*bp_model3_, spawn_point_);
    // auto act_vehicle_model3_ = boost::static_pointer_cast<carla::client::Vehicle>(act_);
    // this->__vehicle_model3 = std::make_shared<boost::shared_ptr<carla::client::Vehicle>(boost::static_pointer_cast<carla::client::Vehicle>(act_))>;
    this->__vehicleModel3 = boost::static_pointer_cast<carla::client::Vehicle>(act_);

    // 移动仿真器的视角到车旁边
    auto transform_view_ = carla::geom::Transform(spawn_point_);
    transform_view_.location +=  -5.0f * transform_view_.GetForwardVector();
    transform_view_.location.z += 2.0f;
    transform_view_.rotation.pitch = -15.0f;
    (*this->__carlaSpectator)->SetTransform(transform_view_);

    // 添加一个图像传感器到车上
    auto cam_rgb_bp_ = bpl_->Find("sensor.camera.rgb");
    assert (cam_rgb_bp_), "找不到蓝图 sensor.camera.rgb";
    auto cam_transform_ = carla::geom::Transform(
        carla::geom::Location(3, 0, 1.2),
        carla::geom::Rotation(-10, 0, 0)
    );
    auto actor_ = this->__carlaWorld->SpawnActor(*cam_rgb_bp_, cam_transform_, this->__vehicleModel3.get());    // 将摄像头放在车上
    this->__camRgb = boost::static_pointer_cast<carla::client::Sensor>(actor_);
    this->__camRgb->Listen([&](auto data) {
        auto image_data_ = boost::static_pointer_cast<carla::sensor::data::Image>(data);
        assert (image_data_), "收到错误的图像数据";
        this->__OnRecvSensorImageData(*image_data_);
    });

    // 设置车辆控制器回调
    this->__rpcVehicleControlSubscriber = 
        this->create_subscription<carla_ros2_interface::msg::RpcVehicleControl>(
            "/rpc_vehicle_control",
            10,
            // [&](const carla_ros2_interface::msg::RpcVehicleControl::SharedPtr msg) {
            //     RCLCPP_INFO(rclcpp::get_logger("__OnRecvVehicleControlRequest"), "收到控车指令");
            //     this->__OnRecvVehicleControlRequest(msg);
            // }
            std::bind(&ManualEgoCar::__OnRecvVehicleControlRequest, this, _1));

    // this->__carlaClient = carla::client::Client("127.0.0.1", 2000);
    // this->__carlaClient.SetTimeout(10s);

}

ManualEgoCar::~ManualEgoCar()
{
    if (this->__vehicleModel3) {
        this->__vehicleModel3->Destroy();
    }
    if (this->__camRgb) {
        this->__camRgb->Destroy();
    }
}

// void ManualEgoCar::__OnRecvRpcVehicleControl(const carla_ros2_interface::msg::RpcVehicleControl::SharedPtr rpc_vehicle_control)
// {
//     // 创建一个车辆控制器 >>>>>>
//     carla::client::Vehicle::Control control_(
//         rpc_vehicle_control->throttle,
//         rpc_vehicle_control->steer,
//         rpc_vehicle_control->brake,
//         rpc_vehicle_control->hand_brake,
//         rpc_vehicle_control->reverse,
//         rpc_vehicle_control->manual_gear_shift,
//         rpc_vehicle_control->gear
//     );
//     this->__vehicleModel3->ApplyControl(control_);
// }

void ManualEgoCar::__OnRecvSensorImageData(const carla::sensor::data::Image& image)
{
    auto img_time_ = image.GetTimestamp();
    auto pc_time_ = std::chrono::system_clock::now().time_since_epoch().count();
}

void ManualEgoCar::__OnRecvVehicleControlRequest(const carla_ros2_interface::msg::RpcVehicleControl::SharedPtr rpc_vehicle_control)
{
    // 创建一个车辆控制器 >>>>>>
    // RCLCPP_INFO(rclcpp::get_logger("__OnRecvVehicleControlRequest"), "收到控车指令");
    carla::client::Vehicle::Control control_(
        rpc_vehicle_control->throttle,
        rpc_vehicle_control->steer,
        rpc_vehicle_control->brake,
        rpc_vehicle_control->hand_brake,
        rpc_vehicle_control->reverse,
        rpc_vehicle_control->manual_gear_shift,
        rpc_vehicle_control->gear
    );
    this->__vehicleModel3->ApplyControl(control_);
}

// void OnRecvSensorImageData(const carla::sensor::data::Image& image)
// {
//     auto img_time_ = image.GetTimestamp();
//     auto pc_time_ = std::chrono::system_clock::now().time_since_epoch().count();
// }